import lejos.nxt.Button;
import lejos.nxt.Motor;

public class MotorController {
	public static final int LEFT = 0;
	public static final int RIGHT = 1;
	
	private Motor[] motors = new Motor[2];
	
	/*
	 * Constructs a new Driver object.
	 * 
	 * @param left Motor that should be used for driving the left wheel.
	 * @param right Motor that should be used for driving the right wheel.
	 */
	public MotorController(Motor left, Motor right) {
		motors[LEFT] = left;
		motors[RIGHT] = right;
		
		setSmoothAcceleration(true);
		setRegulateSpeed(true);
	}
	
	/*
	 * Sets up the motor port for the left and right wheel.
	 * 
	 * @param side Either left or right.
	 * @param motor Motor that should be used for this side. 
	 */
	public void setMotorPort(int side, Motor motor) {
		if (!(side < 0 || side > motors.length)) {
			motors[side] = motor;
		}
	}
	
	/* Computes the needed rotation to travel the specified distance.
	 * 
	 * @param dist Distance the robot needs to travel in centimeters.
	 * @return Amount of degrees the wheel needs to rotate to travel the specified distance.
	 */
	public int computeRotation(double dist) {
		double circumference = Configuration.WHEEL_DIAMETER * Math.PI;
		double rotations = dist / circumference;
		return (int)(rotations * 360);
	}
	
	/*
	 * Computes the distance in centimeters for the given rotation.
	 * 
	 * @param rotation Rotation in degrees.
	 * @return Distance in centimeters.
	 */
	public double computeDistanceForRotation(double rotation) {	
		return (rotation / 360) * Math.PI * Configuration.WHEEL_DIAMETER;
	}
	
	/*
	 * Enables or disables regulateSpeed for both motors.
	 *
	 * @param enabled Desired state of regulateSpeed
	 */
	public void setRegulateSpeed(boolean enabled) {
		for (int i = 0 ; i < motors.length ; i++) {
			motors[i].regulateSpeed(enabled);
		}
	}
	
	/*
	 * Enables or disables smoothAcceleration for both motors.
	 *
	 * @param enabled Desired state of smoothAcceleration
	 */
	public void setSmoothAcceleration(boolean enabled) {
		for (int i = 0 ; i < motors.length ; i++) {
			motors[i].smoothAcceleration(enabled);
		}
	}
	
	/*
	 * Returns the distance travelled since last reset.
	 * 
	 * @return Distance in centimeters.
	 */
	public double getTravelledDistance() {
		int leftTacho = motors[LEFT].getTachoCount();
		int rightTacho = motors[RIGHT].getTachoCount();
		double tacho = (leftTacho + rightTacho) * 0.5;
		
		return computeDistanceForRotation(tacho);
	}
	
	/*
	 * Stops both engines
	 */
	public void stop() {
		motors[LEFT].stop();
		motors[RIGHT].stop();
		try {
			Thread.sleep(250);
		} catch (InterruptedException e) {}
	}
	
	/*
	 * Makes the robot drive (a fraction of) a circle
	 * 
	 * @param speed Speed in cm per second.
	 * @param radius Radius in cm.
	 * @param fraction Fraction of the circle that should be driven.
	 */
	public void drive(double speed, double radius, double fraction) {
		drive(computeRotation(speed), radius, fraction);
	}
	
	/*
	 * Makes the robot drive (a fraction of) a circle
	 * 
	 * @param speed Speed in degrees per second.
	 * @param radius Radius in cm.
	 * @param fraction Fraction of the circle that should be driven.
	 */
	public void drive(int speed, double radius, double fraction) {
		double distInner = (radius - 0.5 * Configuration.WHEEL_BASE) * Math.PI * fraction * (1-Configuration.DRIFT_PCT) * (radius > 0 ? 1 : -1) * (double)(2 * (speed > 0 ? 1 : -1));
		double distOuter = (radius + 0.5 * Configuration.WHEEL_BASE) * Math.PI * fraction * (1-Configuration.DRIFT_PCT) * (radius > 0 ? 1 : -1) * (double)(2 * (speed > 0 ? 1 : -1));
		
		double min = Math.abs(distInner) > Math.abs(distOuter) ? distOuter : distInner;
		double max = min == distInner ? distOuter : distInner;
		
		int speedOuter = speed;
		int speedInner = (int)(min * speedOuter / max);
		
		int rotOuter = computeRotation(distOuter);
		int rotInner = computeRotation(distInner);
		
		if (radius >= 0) {
			drive(speedOuter, speedInner, rotOuter, rotInner);
		} else {
			drive(speedInner, speedOuter, rotOuter, rotInner);
		}
	}
	
	/*
	 * Drives the robot with specified speed and rotation.
	 * 
	 * @param speedLeft	Desired speed for the left wheel in degrees per second.
	 * @param speedRight Desired speed for the right wheel in degrees per second.
	 * @param degreesLeft Desired amount of degrees the left wheel should turn.
	 * @param degreesRight Desired amount of degrees the right wheel should turn.
	 */
	public void drive(int speedLeft, int speedRight, int degreesLeft, int degreesRight) {
		DebugLogger.write("Left Motor - Speed: " + speedLeft + " - Degrees: " + degreesLeft);
		DebugLogger.write("Right Motor - Speed: " + speedRight + " - Degrees: " + degreesRight);
		motors[LEFT].setSpeed(speedLeft);
		motors[RIGHT].setSpeed(speedRight);
		
		motors[LEFT].rotate(degreesLeft, true);
		motors[RIGHT].rotate(degreesRight);
	}
	
	/*
	 * Turns the robot in the given direction at given speed.
	 * 
	 * @param angle Angle the robot should turn relative to its current standing. Negative angles cause a left turn, positive angles a right turn.
	 * @param speed Speed at which the outer wheel should turn in degrees / sec.
	 */
	public void turn(double angle, double speed) {
		int side = angle < 0 ? RIGHT : LEFT;
		
		// Float inner motor.
		motors[1 - side].flt();
		int dist = computeRotation((2*Math.PI*Configuration.WHEEL_BASE)*(Math.abs(angle)/360)*(1-Configuration.DRIFT_PCT));
		motors[side].setSpeed(computeRotation(speed));
		motors[side].rotate(dist);
	}

	public void driveDistance(double distance) {
		int degrees = computeRotation(distance);
		drive(100, 100, degrees, degrees);
	}

	public void resetTacho() {
		motors[LEFT].resetTachoCount();
		motors[RIGHT].resetTachoCount();
	}

	public void forward() {
		DebugLogger.write("Forward");
		motors[LEFT].setSpeed(Configuration.SPEED);
		motors[RIGHT].setSpeed(Configuration.SPEED);
		motors[LEFT].forward();
		motors[RIGHT].forward();
	}

	public void flt(int side) {
		motors[side].flt();
	}
}